﻿using System;
using System.Timers;
using Sharp3D.Math.Core;
using MECopter.Communication;
using MECopter.Configuration.GroundControl;
using MECopter.Configuration.Quadrocopter;
namespace MECopter.Quadrocopter
{
    class Quadrocopter
    {
        #region Fields
        /// <summary>
        /// Actual angels (in radiants)
        /// </summary>
        public Angle YPR{
            get {return _ypr;}
            set {throw new InvalidOperationException();}
        }
        private Angle _ypr = new Angle();

        /// <summary>
        /// Current ESC values
        /// </summary>
        public float[] ESC{
            get {return _esc;}
            set {throw new InvalidOperationException();}
        }
        private float[] _esc = new float[4];        

        /// <summary>
        /// Current loop time on the arduino board
        /// </summary>
        public ulong LoopTime
        {
            get { return _looptime; }
            set {throw new InvalidOperationException();}
        }
        private ulong _looptime = new ulong();


        /// <summary>
        /// Current PID values
        /// </summary>
        public float[] PIDValues
        {
            get { return _pidvalues; }
            set {throw new InvalidOperationException();}
        }
        private float[] _pidvalues = new float[3];

        /// <summary>
        /// Controller of this quadrocopter
        /// </summary>
        public Controller controller;

        public bool IsAvaliable;

        private SerialWrapper serial;

        
        #endregion

        #region Constructor
        public Quadrocopter()
        {            
            IsAvaliable = false;                       
        }
        #endregion

        #region Methods
        public void Enable()
        {
            
            serial = new SerialWrapper();
            serial.Open(ConfigurationManager.CurrentSerialConfig.Port, ConfigurationManager.CurrentSerialConfig.Baudrate);
            serial.ReadBuffer.CollectionChanged += ReadBuffer_CollectionChanged;

            controller = new ManualController();
            controller.startControlling();
            controller.Values.PropertyChanged += Values_PropertyChanged;

            if (serial.Opened)
            {
                
                IsAvaliable = true;
            }
                        
            QuadrocopterConfig.pidParameters.PropertyChanged += pidParameters_PropertyChanged;
            GetPID();            
        }

           

        public void Disable()
        {
            DisableESCs();
            serial.Close();
            IsAvaliable = false;
        }

        public void EnableESCs()
        {            
            serial.WriteQueue.Add(new SendData(SendDataType.ESCEnable, true));
        }

        public void DisableESCs()
        {           
            serial.WriteQueue.Add(new SendData(SendDataType.ESCEnable, false));
        }

        public void SetPID(PIDParameters parameters)
        {          
            serial.WriteQueue.Add(new SendData(SendDataType.PIDParameter, parameters));
            GetPID();
        }

        public void GetPID()
        {            
            serial.WriteQueue.Add(new SendData(SendDataType.Request, ReadDataType.PIDParameter));            
        }
        #endregion


        #region Eventhandler
        //--Output--

        //RC
        void Values_PropertyChanged(object sender, System.ComponentModel.PropertyChangedEventArgs e)
        {
            if (IsAvaliable)
            {
                serial.WriteQueue.Add(new SendData(SendDataType.RC, controller.Values));
            }
        }

        //PID Parameters
        void pidParameters_PropertyChanged(object sender, System.ComponentModel.PropertyChangedEventArgs e)
        {
            SetPID(QuadrocopterConfig.pidParameters);
        } 

        //--Input--
        void ReadBuffer_CollectionChanged(object sender, System.Collections.Specialized.NotifyCollectionChangedEventArgs e)
        {
            if (e.NewItems != null)
            {
                foreach (ReadData readData in e.NewItems)
                {

                    switch (readData.Type)
                    {
                        case ReadDataType.YPR:
                            _ypr = (Angle)readData.Data;
                            break;
                        case ReadDataType.ESC:
                            _esc = (float[])readData.Data;
                            break;
                        case ReadDataType.LoopTime:
                            _looptime = (ulong)readData.Data;
                            break;
                        case ReadDataType.PIDValues:
                            _pidvalues = (float[])readData.Data;
                            break;
                    }
                    serial.ReadBuffer.Remove(readData);
                }
            }
        }

           
        #endregion
    }
}
